/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Kinect;
import edu.wpi.first.wpilibj.Skeleton;
/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class ArmBend extends IterativeRobot {

    Skeleton skeleton;                                                          //The Kinect skeleton is the best way to determine points
    
    float elbowLeftX, elbowLeftY;                                               //These variables are for storing the coordinates of each 
    float elbowRightX, elbowRightY;                                             //critical point in the virtual Kinect coordinate plane.
    float wristLeftX, wristLeftY;                                               //The elbow will be used as the origin on a smaller coordinate
    float wristRightX, wristRightY;                                             //plane. This way, the wrist and shoulder can be used to determine
    float shoulderLeftX, shoulderLeftY;                                         //the angle that the player has his arm bent at. Trigonometry is
    float shoulderRightX, shoulderRightY;                                       //used frequently to determine the angles and such.
    
    int wristLeftQuadrant, wristRightQuadrant;                                  //The quadrant of each point is important when summing up the final
    int shoulderLeftQuadrant, shoulderRightQuadrant;                            //angle
    
    double wristLeftAngle, wristRightAngle;                                     //These hold the relative angles of the wrists and shoulders from
    double shoulderLeftAngle, shoulderRightAngle;                               //180 degrees
    
    double leftAngle;                                                           //The final angles of the arms
    double rightAngle;
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        
        elbowLeftX      = 0;                                                    //Initializers. Fairly self explanatory
        elbowLeftY      = 0;
        elbowRightX     = 0;
        elbowRightY     = 0;
        wristLeftX      = 0;
        wristLeftY      = 0;
        wristRightX     = 0;
        wristRightY     = 0;
        shoulderLeftX   = 0;
        shoulderLeftY   = 0;
        shoulderRightX  = 0;
        shoulderRightY  = 0;
        
        wristLeftQuadrant       = 0;
        wristRightQuadrant      = 0;
        shoulderLeftQuadrant    = 0;
        shoulderRightQuadrant   = 0;
        
        wristLeftAngle      = 0;
        wristRightAngle     = 0;
        shoulderLeftAngle   = 0;
        shoulderRightAngle  = 0;
        
        leftAngle   = 180;                                                      //These start at 180 (degrees) for when the final angle is computed. 
        rightAngle  = 180;                                                      
         
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /** 
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        
        if (Kinect.getInstance().getNumberOfPlayers() > 1) {
            System.out.println("It appears that we have multiple players in the Kinect's field of view...");
        } else {
            getValues();                                                        //Don't expect either of these to be fast...
            calculateAngles();
            System.out.println("Left Arm: " + leftAngle + " Right Arm: " + rightAngle);
        }           
    }
    
    /**
     * Gets the Kinect skeleton, then retrieves the values required from it.
     */
    public void getValues() {
        
        skeleton        = Kinect.getInstance().getSkeleton();                   //Retrieve the current Kinect skeleton from the driver station
        
        elbowLeftX      = skeleton.GetElbowLeft()     .getX();                  //Determine all of the points from the skeleton. Y values are 
        elbowLeftY      = skeleton.GetElbowLeft()     .getY();                  //multiplied by -1 to keep a more human-friendly coordinate system
        elbowRightX     = skeleton.GetElbowRight()    .getX();
        elbowRightY     = skeleton.GetElbowRight()    .getY();
        wristLeftX      = skeleton.GetWristLeft()     .getX();
        wristLeftY      = skeleton.GetWristLeft()     .getY();
        wristRightX     = skeleton.GetWristRight()    .getX();
        wristRightY     = skeleton.GetWristRight()    .getY();
        shoulderLeftX   = skeleton.GetShoulderLeft()  .getX();
        shoulderLeftY   = skeleton.GetShoulderLeft()  .getY();
        shoulderRightX  = skeleton.GetShoulderRight() .getX();
        shoulderRightY  = skeleton.GetShoulderRight() .getY();       
        
        if (wristLeftX > elbowLeftX && wristLeftY > elbowLeftY) {               //Determining the quadrants of the wrists and shoulders relative
            wristLeftQuadrant = 1;                                              //to the elbow which is the origin
        } else if (wristLeftX < elbowLeftX && wristLeftY > elbowLeftY) {
            wristLeftQuadrant = 2;
        } else if (wristLeftX < elbowLeftX && wristLeftY < elbowLeftY) {
            wristLeftQuadrant = 3;
        } else if (wristLeftX > elbowLeftX && wristLeftY < elbowLeftY) {
            wristLeftQuadrant = 4;
        }
        
        if (wristRightX > elbowRightX && wristRightY > elbowRightY) {
            wristRightQuadrant = 1;
        } else if (wristRightX < elbowRightX && wristRightY > elbowRightY) {
            wristRightQuadrant = 2;
        } else if (wristRightX < elbowRightX && wristRightY < elbowRightY) {
            wristRightQuadrant = 3;
        } else if (wristRightX > elbowRightX && wristRightY < elbowRightY) {
            wristRightQuadrant = 4;
        }
        
        if (shoulderLeftY > elbowLeftY) {                                       //If all is well, the shoulders on either side should only fall into
            shoulderLeftQuadrant = 1;                                           //one of two quadrants
        } else if (shoulderLeftY < elbowLeftY) {
            shoulderLeftQuadrant = 4;
        }
        
        if (shoulderRightY > elbowRightY) {
            shoulderRightQuadrant = 2;
        } else if (shoulderRightY < elbowRightY) {
            shoulderRightQuadrant = 3;
        }
        
    }
    
    public void calculateAngles() {
        
        leftAngle   = 180;
        rightAngle  = 180;
        
        wristLeftAngle      = MathUtils.atan(wristLeftY/wristLeftX);            //Retrieve the angles of the points from the origin using some 
        wristRightAngle     = MathUtils.atan(wristRightY/wristRightX);          //trigonometry. Inverse Tangent serves the purpose perfectly.
        shoulderLeftAngle   = MathUtils.atan(shoulderLeftY/shoulderLeftX);
        shoulderRightAngle  = MathUtils.atan(shoulderRightY/shoulderRightX);
        
        wristLeftAngle      = Math.toDegrees(wristLeftAngle);                   //The inverse tangent function returns a value in radians, so it is
        wristRightAngle     = Math.toDegrees(wristRightAngle);                  //wise to change those values to degrees using a function so kindly
        shoulderLeftAngle   = Math.toDegrees(shoulderLeftAngle);                //provided by FRC. The accuracy may be off, but for our purposes,
        shoulderRightAngle  = Math.toDegrees(shoulderRightAngle);               //it should be accurate enough (We'll call it human error...)
        
        switch (wristLeftQuadrant) {                                            //The angles that we received earlier should be added or subtracted
            case 1:                                                             //to 180 (degrees) in order to derive an angle measure. Addition
                leftAngle = leftAngle - (180 - wristLeftAngle);                 //or subtraction is determined by the quadrant that the body part is 
                break;                                                          //in. Then we can find out the angle that the arm is bending at by 
            case 2:                                                             //using the degree measures we got for the wrists and shoulders
                leftAngle = leftAngle - wristLeftAngle;
                break;
            case 3:
                leftAngle = leftAngle + wristLeftAngle;
                break;
            case 4:
                leftAngle = leftAngle + (180 - wristLeftAngle);
                break;
        }
        
        switch (shoulderLeftQuadrant) {
            case 1:
                leftAngle = leftAngle - shoulderLeftAngle;
                break;
            case 4:
                leftAngle = leftAngle + shoulderLeftAngle;
                break;
        }
 
        switch (wristRightQuadrant) {
            case 1:
                rightAngle = rightAngle - wristRightAngle;
                break;
            case 2:
                rightAngle = rightAngle - (180 - wristRightAngle);
                break;
            case 3:
                rightAngle = rightAngle + (180 - wristRightAngle);
                break;
            case 4:
                rightAngle = rightAngle + wristRightAngle;
                break;
        }
        
        switch (shoulderRightQuadrant) {
            case 2:
                rightAngle = rightAngle - shoulderRightAngle;
                break;
            case 3:
                rightAngle = rightAngle + shoulderRightAngle;
                break;
        }
        
    }
    
}
